
#include "opencv2/opencv.hpp"

#include "MathUtil.h"
#include "TagDetection.h"

#ifdef PLATFORM_APERIOS
// missing/broken isnan
namespace std {
static bool isnan(float x) {
  const int EXP = 0x7f800000;
  const int FRAC = 0x007fffff;
  const int y = *((int *)(&x));
  return ((y & EXP) == EXP && (y & FRAC) != 0);
}
}  // namespace std
#endif

namespace AprilTags {

TagDetection::TagDetection()
    : good(false),
      obsCode(),
      code(),
      id(),
      hammingDistance(),
      rotation(),
      p(),
      cxy(),
      observedPerimeter(),
      homography(),
      hxy() {
  homography.setZero();
}

TagDetection::TagDetection(int _id)
    : good(false),
      obsCode(),
      code(),
      id(_id),
      hammingDistance(),
      rotation(),
      p(),
      cxy(),
      observedPerimeter(),
      homography(),
      hxy() {
  homography.setZero();
}

float TagDetection::getXYOrientation() const {
  // Because the order of segments in a quad is arbitrary, so is the
  // homography's rotation, so we can't determine orientation directly
  // from the homography.  Instead, use the homography to find two
  // bottom corners of a properly oriented tag in pixel coordinates,
  // and then compute orientation from that.
  std::pair<float, float> p0 = interpolate(-1, -1);  // lower left corner of tag
  std::pair<float, float> p1 = interpolate(1, -1);  // lower right corner of tag
  float orient = atan2(p1.second - p0.second, p1.first - p0.first);
  return !std::isnan(float(orient)) ? orient : 0.;
}

std::pair<float, float> TagDetection::interpolate(float x, float y) const {
  float z = homography(2, 0) * x + homography(2, 1) * y + homography(2, 2);
  if (z == 0)
    return std::pair<float, float>(0, 0);  // prevents returning a pair with a
                                           // -NaN, for which gcc 4.4 flubs
                                           // isnan
  float newx =
      (homography(0, 0) * x + homography(0, 1) * y + homography(0, 2)) / z +
      hxy.first;
  float newy =
      (homography(1, 0) * x + homography(1, 1) * y + homography(1, 2)) / z +
      hxy.second;
  return std::pair<float, float>(newx, newy);
}

bool TagDetection::overlapsTooMuch(const TagDetection &other) const {
  // Compute a sort of "radius" of the two targets. We'll do this by
  // computing the average length of the edges of the quads (in
  // pixels).
  float radius =
      (MathUtil::distance2D(p[0], p[1]) + MathUtil::distance2D(p[1], p[2]) +
       MathUtil::distance2D(p[2], p[3]) + MathUtil::distance2D(p[3], p[0]) +
       MathUtil::distance2D(other.p[0], other.p[1]) +
       MathUtil::distance2D(other.p[1], other.p[2]) +
       MathUtil::distance2D(other.p[2], other.p[3]) +
       MathUtil::distance2D(other.p[3], other.p[0])) /
      16.0f;

  // distance (in pixels) between two tag centers
  float dist = MathUtil::distance2D(cxy, other.cxy);

  // reject pairs where the distance between centroids is smaller than
  // the "radius" of one of the tags.
  return (dist < radius);
}

Eigen::Matrix4d TagDetection::getRelativeTransform(double tag_size, double fx,
                                                   double fy, double px,
                                                   double py) const {
  std::vector<cv::Point3f> objPts;
  std::vector<cv::Point2f> imgPts;
  double s = tag_size / 2.;
  objPts.push_back(cv::Point3f(-s, -s, 0));
  objPts.push_back(cv::Point3f(s, -s, 0));
  objPts.push_back(cv::Point3f(s, s, 0));
  objPts.push_back(cv::Point3f(-s, s, 0));

  std::pair<float, float> p1 = p[0];
  std::pair<float, float> p2 = p[1];
  std::pair<float, float> p3 = p[2];
  std::pair<float, float> p4 = p[3];
  imgPts.push_back(cv::Point2f(p1.first, p1.second));
  imgPts.push_back(cv::Point2f(p2.first, p2.second));
  imgPts.push_back(cv::Point2f(p3.first, p3.second));
  imgPts.push_back(cv::Point2f(p4.first, p4.second));

  cv::Mat rvec, tvec;
  cv::Matx33f cameraMatrix(fx, 0, px, 0, fy, py, 0, 0, 1);
  cv::Vec4f distParam(0, 0, 0, 0);  // all 0?
  cv::solvePnP(objPts, imgPts, cameraMatrix, distParam, rvec, tvec);
  cv::Matx33d r;
  cv::Rodrigues(rvec, r);
  Eigen::Matrix3d wRo;
  wRo << r(0, 0), r(0, 1), r(0, 2), r(1, 0), r(1, 1), r(1, 2), r(2, 0), r(2, 1),
      r(2, 2);

  Eigen::Matrix4d T;
  T.topLeftCorner(3, 3) = wRo;
  T.col(3).head(3) << tvec.at<double>(0), tvec.at<double>(1),
      tvec.at<double>(2);
  T.row(3) << 0, 0, 0, 1;

  return T;
}

void TagDetection::getRelativeTranslationRotation(double tag_size, double fx,
                                                  double fy, double px,
                                                  double py,
                                                  Eigen::Vector3d &trans,
                                                  Eigen::Matrix3d &rot) const {

  Eigen::Matrix4d T = getRelativeTransform(tag_size, fx, fy, px, py);

  // converting from camera frame (z forward, x right, y down) to
  // object frame (x forward, y left, z up)
  Eigen::Matrix4d M;
  M << 0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1;
  Eigen::Matrix4d MT = M * T;
  // translation vector from camera to the April tag
  trans = MT.col(3).head(3);
  // orientation of April tag with respect to camera: the camera
  // convention makes more sense here, because yaw,pitch,roll then
  // naturally agree with the orientation of the object
  rot = T.block(0, 0, 3, 3);
}

// draw one April tag detection on actual image
void TagDetection::draw(cv::Mat &image) const {
  // use corner points detected by line intersection
  std::pair<float, float> p1 = p[0];
  std::pair<float, float> p2 = p[1];
  std::pair<float, float> p3 = p[2];
  std::pair<float, float> p4 = p[3];

  // plot outline
  cv::line(image, cv::Point2f(p1.first, p1.second),
           cv::Point2f(p2.first, p2.second), cv::Scalar(255, 0, 0, 0));
  cv::line(image, cv::Point2f(p2.first, p2.second),
           cv::Point2f(p3.first, p3.second), cv::Scalar(0, 255, 0, 0));
  cv::line(image, cv::Point2f(p3.first, p3.second),
           cv::Point2f(p4.first, p4.second), cv::Scalar(0, 0, 255, 0));
  cv::line(image, cv::Point2f(p4.first, p4.second),
           cv::Point2f(p1.first, p1.second), cv::Scalar(255, 0, 255, 0));

  // mark center
  cv::circle(image, cv::Point2f(cxy.first, cxy.second), 8,
             cv::Scalar(0, 0, 255, 0), 2);

  // print ID
  std::ostringstream strSt;
  strSt << "#" << id;
  cv::putText(image, strSt.str(), cv::Point2f(cxy.first + 10, cxy.second + 10),
              cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 0, 255));
}

}  // namespace AprilTags
